﻿using System;
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
namespace COC.Tools.Pathing2
{
    public sealed class AIPathfinding : MonoBehaviour
    {


        DateTime startTime;
        TimeSpan consumedTime;
        #region tileParameters
        public float mTileSize { get; set; }
        public float mOffset { get; set; }

        public CTile[] mBHVArray { get; set; }
        public Transform mAnsestor { get; set; }
        public float mRows { get; set; }
        public float mColumns { get; set; }
        public float mHeight { get; set; }

        #endregion

        #region properties
        List<CTile> openList = new List<CTile>();
        Dictionary<int,GraphNodeInfo> followersGraphNodeInfo = new Dictionary<int,GraphNodeInfo>();//save each follower's owned graph nodes informations into list
        public Dictionary<int,PathInfo> followersPathInfo = new Dictionary<int,PathInfo>();//save each follower's owned path informations into list
        public byte walkable = 0x0;
        public byte unWalkable = 0x1;
        byte unVisited = 0x02;
        byte visited = 0x3;
        byte onOpenList = 0x0;
        public bool stopped = false;
        const int found = 1, nonexistent = 2;
        Dictionary<int, int> storedPathFinderPathLength = new Dictionary<int, int>();
        int pathStatus = 0;
        #endregion
        #region a* pathfinding internal field
        Dictionary<int, byte[,]> wishList = new Dictionary<int, byte[,]>();//key=chase value which point have already  accessed.

        public CTile[,] mObstacleMap;

        #endregion
        #region a star pathing function
        public IEnumerator FindPath(int followerId, Transform start, Transform target)
        {
            string s = "";//print unvisited node on screen. 
            int startRow = (int)(start.localPosition.x / (mTileSize + mOffset)) + 1;
            int startCol = (int)(start.localPosition.z / (mTileSize + mOffset)) + 1;
            int targetRow = (int)(target.localPosition.x / (mTileSize + mOffset)) + 1;
            int targetCol = (int)(target.localPosition.z / (mTileSize + mOffset)) + 1;
            PathInfo pathInfo;
            var graphNodeInfo = initializeRequiredParameters(followerId, out pathInfo);
            float startingX = Mathf.Round((startRow * (mTileSize + mOffset)));
            float startingY = Mathf.Round((startCol * (mTileSize + mOffset)));
            int a = 0;
            int b = 0;
            Vector3 startNewPos = new Vector3(target.position.x, target.position.y, target.position.z);
            var time = 1.0f;
            int path = 0;
            float speed = 1.0f / time;
           // while (start.position != startNewPos)
           // {
            //    start.localPosition = Vector3.MoveTowards(start.localPosition, startNewPos, speed);
             //   yield return null;
           // }
            float destX = Mathf.Round((targetRow * (mTileSize + mOffset)));
            float destY = Mathf.Round((targetCol * (mTileSize + mOffset)));
            graphNodeInfo.numberOfOpenListItems = 1;
            /*
                    for (int i = 0; i<mBHVArray.Length; i++)
                    {
                        if(Math.Abs(mBHVArray[i].mOrigin.x - startingX) < 1&&Math.Abs(mBHVArray[i].mOrigin.z - startingY) < 1)
                        {
                            UnityEngine.Debug.Log("index at:=" + mBHVArray[i].index+ " has a vector [" + mBHVArray[i].mOrigin.x+","+mBHVArray[i].mOrigin.z+"] which location ["+mBHVArray[i].pointX+","+mBHVArray[i].pointY+"]contained of "+"["+start.localPosition.x+","+start.localPosition.z+"].");
                        }
                        yield return null;
                    }
                   */
            //	If starting location and target are in the same location...
            if (Math.Abs(startingX - destX) < 1 && Math.Abs(startingY - destY) < 1 && followersPathInfo[followerId].location > 0)
            {

                pathInfo.STATE = PathInfo.PathState.ONE;
            }
            if (Math.Abs(startingX - destX) < 1 && Math.Abs(startingY - destY) < 1 && followersPathInfo[followerId].location == 0)
            {

                pathInfo.STATE = PathInfo.PathState.NONE;
            }
            if (mObstacleMap[targetRow - 1, targetCol - 1].walkable == unWalkable)
            {
                //TODO
                //notify to inform this target locaiton is not walkable. 
                pathInfo.STATE = PathInfo.PathState.NONE;
                
                yield break;
            }
            if (Math.Abs(startingX - destX) > 1 || Math.Abs(startingY - destY) > 1)
            {

                byte[,] whichList = new byte[(int)mRows, (int)mColumns]; // dimensional array used to record 
                // 		whether a cell is on the open list or on the closed list.
                graphNodeInfo.mGCost = new int[(int)mRows, (int)mColumns];
                graphNodeInfo.mHCost = new int[(int)mRows * (int)mColumns + 2];
                graphNodeInfo.mFCost = new int[(int)mRows * (int)mColumns + 2];
                graphNodeInfo.mParent = new GraphNodeInfo.NodeParent[(int)mRows, (int)mColumns + 2];
                wishList.Add(followerId, whichList);

                Dictionary<int, int[]> openList = new Dictionary<int, int[]>(); //1 dimensional array holding ID# of open list items
                int u, v;
                u = v = 1;
                int parentX = startRow;
                int parentY = startCol;
                int m = 1;
                int newOpenListItemID = 0;
                int temp = 0;
                byte corner = 0x0;
                int tempGcost = 0;
                int pathX = 0;
                int pathY = 0;
                int tempX = 0;
                graphNodeInfo.mGCost[parentX, parentY] = 0;
                int[] tempXY = new int[2];
                whichList[startRow, startCol] = visited;//first,check the first node( ID#1 ) have already accessed.
                startTime = System.DateTime.Now;

                do
                {


                    print("follower["+followerId+"]  mGraphNodeInfo.numberOfOpenListItems =" + graphNodeInfo.numberOfOpenListItems);
                    //If the open list is not empty, take the first cell off of the list.
                    //	This is the lowest F cost cell on the open list.
                    if (graphNodeInfo.numberOfOpenListItems != 0)
                    {
                        // Pop the first item off the open list.
                        whichList[parentX, parentY] = visited;
                        graphNodeInfo.numberOfOpenListItems = graphNodeInfo.numberOfOpenListItems - 1; ///reduce number of open list items by 1	
                        //	Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
                        openList.TryGetValue(graphNodeInfo.numberOfOpenListItems + 1, out tempXY); ;//move the last item in the heap up to slot #1
                        if (tempXY != null)
                        {
                            parentX = tempXY[0];
                            parentY = tempXY[1];
                           // Debug.Log("follower["+followerId+"] parentX = " + tempXY[0] + ";" + "parentY = " + tempXY[1]);

                        }

                        /*  if (u * 2 + 1 <= numberOfOpenListItems)//if exsit both child
                          {
                       
                          }*/


                        int addedGCost = 0;
                        for (a = parentX - 1; a <= parentX + 1; a++)
                        {
                            for (b = parentY - 1; b <= parentY + 1; b++)
                            {
                                //1.we need a valid tile
                                if (a != -1 && b != -1 && a != mRows && b != mColumns)
                                {
                                    //2.we need to put a unvisited position xy or a none-opened position into binary heap.
                                    if (whichList[a, b] != visited)
                                    {
                                        //3.if not obstacles
                                        if (mObstacleMap[a, b].walkable != unWalkable)
                                        {
                                            //	Don't cut across corners
                                            corner = walkable;
                                            if (a == parentX - 1)
                                            {
                                                if (b == parentY - 1)
                                                {
                                                    if (mObstacleMap[parentX - 1, parentY].walkable == unWalkable
                                                        || mObstacleMap[parentX, parentY - 1].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                                else if (b == parentY + 1)
                                                {
                                                    if (mObstacleMap[parentX, parentY + 1].walkable == unWalkable
                                                        || mObstacleMap[parentX - 1, parentY].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                            }
                                            else if (a == parentX + 1)
                                            {
                                                if (b == parentY - 1)
                                                {
                                                    if (mObstacleMap[parentX, parentY - 1].walkable == unWalkable
                                                        || mObstacleMap[parentX + 1, parentY].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                                else if (b == parentY + 1)
                                                {
                                                    if (mObstacleMap[parentX + 1, parentY].walkable == unWalkable
                                                        || mObstacleMap[parentX, parentY + 1].walkable == unWalkable)
                                                        corner = unWalkable;
                                                }
                                            }
                                            if (corner != unWalkable)
                                            {
                                                //4.Create a new open list item in the binary heap.
                                                if (whichList[a, b] != unVisited)
                                                {
                                                    newOpenListItemID = newOpenListItemID + 1; //each new item has a unique ID #
                                                    m = graphNodeInfo.numberOfOpenListItems + 1;

                                                    openList[m] = new int[] { a, b };//record coordinate x y for new item 
                                                    if (Mathf.Abs(a - parentX) == 1 && Mathf.Abs(b - parentY) == 1)
                                                    {
                                                        addedGCost = 14;
                                                    }
                                                    else
                                                    {
                                                        addedGCost = 10;
                                                    }

                                                    graphNodeInfo.mGCost[a, b] = (graphNodeInfo.mGCost[parentX, parentY] + addedGCost);
                                                    graphNodeInfo.mParent[a, b] = new GraphNodeInfo.NodeParent();
                                                    graphNodeInfo.mHCost[newOpenListItemID] = 10 * (Mathf.Abs(a - targetRow - 1) + Mathf.Abs(b - targetCol - 1));
                                                    graphNodeInfo.mParent[a, b].x = parentX;
                                                    graphNodeInfo.mParent[a, b].y = parentY;
                                                    graphNodeInfo.mFCost[m] = graphNodeInfo.mHCost[m] + graphNodeInfo.mGCost[a, b];
                                                    //2.Move the new open list item to the proper place in the binary heap.
                                                    //Starting at the bottom, successively compare to parent items,
                                                    //swapping as needed until the item finds its place in the heap
                                                    //or bubbles all the way to the top (if it has the lowest F cost). 
                                                    while (m != 1)
                                                    {

                                                        if (graphNodeInfo.mFCost[m] < graphNodeInfo.mFCost[m / 2])
                                                        {
                                                            temp = graphNodeInfo.mFCost[m / 2];
                                                            graphNodeInfo.mFCost[m / 2] = graphNodeInfo.mFCost[m];
                                                            graphNodeInfo.mFCost[m] = temp;
                                                            m /= 2;
                                                        }
                                                        else
                                                            break;

                                                    }
                                                    graphNodeInfo.numberOfOpenListItems = graphNodeInfo.numberOfOpenListItems + 1;//add one to the number of items in the heap

                                                    //Change whichList to show that the new item is on the open list.
                                                    whichList[a, b] = unVisited;

                                                   // Debug.Log("follower[" + followerId + "] whichList[" + a + "," + b + "] = " + (s = (whichList[a, b] == unVisited) ? "unVisited" : ""));

                                                }
                                                else
                                                {
                                                    ////8.If adjacent cell is already on the open list, check to see if this 
                                                    //	path to that cell from the starting location is a better one. 
                                                    //	If so, change the parent of the cell and its G and F costs.	
                                                    //Figure out the G cost of this possible new path
                                                    if (Mathf.Abs(a - parentX) == 1 && Math.Abs(b - parentY) == 1)
                                                        addedGCost = 14;//cost of going to diagonal tiles	
                                                    else
                                                        addedGCost = 10;//cost of going to non-diagonal tiles				
                                                    tempGcost = graphNodeInfo.mGCost[parentX, parentY] + addedGCost;
                                                    //If this path is shorter (G cost is lower) then change
                                                    //the parent cell, G cost and F cost. 	
                                                    if (tempGcost < graphNodeInfo.mGCost[a, b])
                                                    {
                                                        graphNodeInfo.mParent[a, b].x = parentX;
                                                        graphNodeInfo.mParent[a, b].y = parentY;
                                                        graphNodeInfo.mGCost[a, b] = tempGcost;
                                                        int[] openXYValue = null;
                                                        for (int x = 1; x <= graphNodeInfo.numberOfOpenListItems; x++)
                                                        {

                                                            openList.TryGetValue(x, out openXYValue);
                                                            if (openXYValue != null && openXYValue[0] == a && openXYValue[1] == b)
                                                            {
                                                                graphNodeInfo.mFCost[x] = graphNodeInfo.mGCost[a, b] + graphNodeInfo.mHCost[x];
                                                                m = x;
                                                                //See if changing the F score bubbles the item up from it's current location in the heap
                                                                while (m != 1)//While item hasn't bubbled to the top (m=1)	
                                                                {
                                                                    //Check if child is < parent. If so, swap them.	
                                                                    if (graphNodeInfo.mFCost[m] < graphNodeInfo.mFCost[m / 1])
                                                                    {
                                                                        temp = graphNodeInfo.mFCost[m >> 1];
                                                                        graphNodeInfo.mFCost[m >> 1] = graphNodeInfo.mFCost[m];
                                                                        graphNodeInfo.mFCost[m] = temp;
                                                                        m /= 2;

                                                                    }
                                                                    else break;


                                                                }

                                                                break;
                                                            }
                                                        }

                                                    }

                                                }
                                            }

                                        }

                                    }
                                }

                            }

                        }
                        //if exist a target position x z in whichList,then found it
                        if (whichList[targetRow, targetCol] == unVisited)
                        {
                            UnityEngine.Debug.Log("follower[" + followerId + "] targetRow=" + targetRow + ",targetCol=" + targetCol + "have found");
                            pathInfo.STATE = PathInfo.PathState.ONE;
                            break;
                        }
                    }
                    else
                    {
                        pathInfo.STATE = PathInfo.PathState.NONE;
                        consumedTime = System.DateTime.Now - startTime;
                        UnityEngine.Debug.Log("follower[" + followerId + "] targetRow=" + targetRow + ",targetCol=" + targetCol + "have not found.consumed time:" + String.Format("{0}.{1}", consumedTime.Seconds, consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
                        yield break;
                    }

                } while (!stopped);
                if (pathInfo.STATE == PathInfo.PathState.ONE)
                {
                    //a.Working backwards from the target to the starting location by checking
                    //	each cell's parent, figure out the length of the path.
                    consumedTime = System.DateTime.Now - startTime;
                    UnityEngine.Debug.Log("follower[" + followerId + "] I'm found target." + "[" + target.position.ToString() + "]" + " consumed time:" + String.Format("{0}.{1}", consumedTime.Seconds, consumedTime.Milliseconds.ToString().PadLeft(3, '0')));
                    pathX = targetRow; pathY = targetCol;
                    storedPathFinderPathLength.Add(followerId, 0);
                    int pathLength = 0;
                    int cellPositionX = 0;
                    int cellPositionY = 0;
                    do
                    {
                        tempX = graphNodeInfo.mParent[pathX, pathY].x;
                        pathY = graphNodeInfo.mParent[pathX, pathY].y;
                        pathX = tempX;
                        //Figure out the path length
                        pathInfo.length = pathInfo.length + 1;
                    } while (pathX != startRow || pathY != startCol);
                    //b.Now copy the path information over to the databank. Since we are
                    //	working backwards from the target to the start location, we copy
                    //	the information to the data bank in reverse order. The result is
                    //	a properly ordered set of path data, from the first step to the
                    //	last.
                    pathX = targetRow; pathY = targetCol;
                    cellPositionX = pathInfo.length * 2;//start at the end	
                    pathInfo.pathBank.Clear();
                    do
                    {
                        cellPositionX = cellPositionX - 2;//work backwards 2 integers
                        UnityEngine.Debug.Log("------------------------->>>>>cellPositionX :pathX,pathY[" + pathX + "," + pathY + "]  <<<<----------------------------");
                        pathInfo.pathBank.Add(new float[]{pathX,pathY});
                     

                        //c.Look up the parent of the current cell.	
                        tempX = graphNodeInfo.mParent[pathX, pathY].x;
                        pathY = graphNodeInfo.mParent[pathX, pathY].y;
                        pathX = tempX;

                        //d.If we have reached the starting square, exit the loop.	
                    }
                    
                    while (pathX != startRow || pathY != startCol);
                    //-----------------------DEBUG-----------------------------
                    //draw line on screen if enable debug 
                    if(false)
                    {
                    float[] curr =null;
                    float[] next = null;
                        for (int d =0; d < pathInfo.pathBank.Count-1; d++)
                        {
                           
                            curr=pathInfo.pathBank[d];
                            next = pathInfo.pathBank[d + 1];
                            UnityEngine.Debug.DrawLine(new Vector3(curr[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f, curr[1] + (mTileSize + mOffset) + (mTileSize / 2)), new Vector3(next[0] + (mTileSize + mOffset) + (mTileSize / 2), 1.5f, next[1] + (mTileSize + mOffset) + (mTileSize / 2)), Color.green);

                        }
                       
                    }
                    //e.mark this shorter path for follower

                    UnityEngine.Debug.Log("starting to move");
                    ReadPath(followerId, startingX, startingY);

                }
            }



            yield return null;
        }

        private GraphNodeInfo initializeRequiredParameters(int followerId, out PathInfo pathInfo)
        {
            GraphNodeInfo graphNodeInfo = new GraphNodeInfo();
            PathInfo cachePathInfo;
            GraphNodeInfo cacheGraphNode; 
            followersPathInfo.TryGetValue(followerId, out cachePathInfo);
            followersGraphNodeInfo.TryGetValue(followerId,out cacheGraphNode);
            if (cachePathInfo != null)
                followersPathInfo.Remove(followerId);
            if (cacheGraphNode != null)
                followersGraphNodeInfo.Remove(followerId);
            pathInfo = new PathInfo();
            followersPathInfo.Add(followerId, pathInfo);
            followersGraphNodeInfo.Add(followerId, graphNodeInfo);
            return graphNodeInfo;
        }

        public int step = 0;
        public Vector3 ReadPath(int followerId, float startingX, float startingZ)
        {
            step++;
            GameObject go = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
            go.name="follower(Clone)_"+step;
            go.transform.position=new Vector3(startingX,1.49f,startingZ);
            go.transform.localScale = new Vector3(mTileSize / 3, .11f, mTileSize / 3);
            go.renderer.material.color=Color.clear;
            if (followersPathInfo[followerId].STATE == PathInfo.PathState.ONE)
            {
           // Debug.Log("step:=" + step);
                if (followersPathInfo[followerId].location < followersPathInfo[followerId].length)
                {
                   // if(followersPathInfo[followerId].location==0)
                   // {
                        followersPathInfo[followerId].location++;
                     
                    //}

                    followersPathInfo[followerId].currPosition = ReadPathXZ(followerId, followersPathInfo[followerId].location);
                    if (followersPathInfo[followerId].location == followersPathInfo[followerId].length)
                    {
                        followersPathInfo[followerId].STATE=PathInfo.PathState.NONE;
                    }
                }
                
            }
            else{
                followersPathInfo[followerId].currPosition = Vector3.right * startingX+Vector3.up*mHeight+Vector3.forward*startingZ;
            }
            return followersPathInfo[followerId].currPosition;
        }
        public Vector3 ReadPathXZ(int followerId,int pathLocation)
        {
            Vector3 xyz=Vector3.zero;
            float[] xz = null;
            UnityEngine.Debug.Log("location:=" + followersPathInfo[followerId].location);
           // Debug.Log("followersPathInfo[followerId].pathBank.Count - 1 - pathLocation =" + (followersPathInfo[followerId].pathBank.Count - 1 - pathLocation));
            if(pathLocation <=followersPathInfo[followerId].length)
            {
                if (followersPathInfo[followerId].pathBank.Count - 1 - pathLocation >= 0)
                {

                    xz = followersPathInfo[followerId].pathBank[(followersPathInfo[followerId].pathBank.Count - 1 - pathLocation)];

                    xz[0] = xz[0] * (mTileSize + mOffset)+mTileSize/2+.45f;
                    xz[1] = xz[1] * (mTileSize + mOffset)+mTileSize/2+.45f;
                    xyz = new Vector3(xz[0],mHeight,xz[1]);
                    UnityEngine.Debug.Log("STEP:" + (step++) + "------------------------------>>>>>>>>>>>>>>>>>>xyz=[" + followersPathInfo[followerId].pathBank[(followersPathInfo[followerId].pathBank.Count - 1 - pathLocation)][0] + "," + mHeight + "," + followersPathInfo[followerId].pathBank[(followersPathInfo[followerId].pathBank.Count - 1 - pathLocation)][1] + "] <<<<<<<<<<<<<<<<<<<-------------------------------");
                }

            }
                return xyz;
        }

       
        #endregion

        void Destroy()
        {
            if (this.gameObject.GetComponent<AIPathfinding>() != null)
                StopCoroutine("FindPath");
        }
    }
}